/*
 * RangbheruFirmware.ino
 * XIAO ESP32S3 Sense + Wio-SX1262
 *
 * Libraries required (install via Arduino Library Manager):
 *   NimBLE-Arduino          by h2zero
 *   FastLED                 by Daniel Garcia   (>= 3.6.0, replaces Adafruit NeoPixel)
 *   Adafruit SSD1306        by Adafruit
 *   Adafruit GFX Library    by Adafruit
 *   arduinoFFT              by Enrique Condes
 *   RadioLib                by Jan Gromeš
 *
 * Board: XIAO_ESP32S3 (Seeed Studio XIAO ESP32S3)
 */

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <NimBLEDevice.h>
#include <FastLED.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <math.h>

// ─────────────────────────────────────────────────────────────────────────────
// FEATURE FLAGS  — set to true one-by-one as you test each module
// ─────────────────────────────────────────────────────────────────────────────
#define ENABLE_LORA    false   // set true later for A2 mesh testing
#define ENABLE_AUDIO   true    // Beat Sync audio reactive (INMP441 I2S mic)
#define ENABLE_MAG     false   // set true for compass / mesh test

#if ENABLE_AUDIO
  #include    // new ESP-IDF 5.x I2S API (avoids conflict with FastLED)
  #include 
#endif

#if ENABLE_LORA
  #include 
#endif

// ─────────────────────────────────────────────────────────────────────────────
// PIN DEFINES
// ─────────────────────────────────────────────────────────────────────────────
#define I2C_SDA          5     // D4
#define I2C_SCL          6     // D5
#define NEOPIXEL_PIN     2     // D1  (via 499Ω)
#define NEOPIXEL_COUNT   34    // 17 front + 17 back (waist)
#define LED_TYPE         WS2812B
#define COLOR_ORDER      GRB
#define MAX_MA           1500  // FastLED current cap (mA) — lower for thin conductive thread

// ── Ring layout (0-based FastLED indices) ────────────────────────────────────
// Front: outer circle first in chain → inner → center
#define FRONT_OUTER_START  0
#define FRONT_OUTER_END    11  // 12 pixels
#define FRONT_INNER_START  12
#define FRONT_INNER_END    15  // 4 pixels
#define FRONT_CENTER       16  // 1 pixel
// Back: outer circle continues chain → inner → center
#define BACK_OUTER_START   17
#define BACK_OUTER_END     28  // 12 pixels
#define BACK_INNER_START   29
#define BACK_INNER_END     32  // 4 pixels
#define BACK_CENTER        33  // 1 pixel
#define BUZZER_PIN       3     // D2

#define I2S_SCK_PIN      1     // D0
#define I2S_WS_PIN       43    // D6
#define I2S_SD_PIN       44    // D7

#define LORA_SCK         8
#define LORA_MISO        9
#define LORA_MOSI        10
#define LORA_CS          41
#define LORA_RST         42
#define LORA_DIO1        45
#define LORA_BUSY        46

// ─────────────────────────────────────────────────────────────────────────────
// DEVICE IDENTITY
// ─────────────────────────────────────────────────────────────────────────────
#define DEVICE_NAME      "Bheru A1"   // change to "Bheru A2" for second device

// ─────────────────────────────────────────────────────────────────────────────
// BLE UUIDs
// ─────────────────────────────────────────────────────────────────────────────
#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHAR_COLOR_UUID     "beb5483e-36e1-4688-b7f5-ea07361b26a8"
#define CHAR_BRI_UUID       "beb5483e-36e1-4688-b7f5-ea07361b26a9"
#define CHAR_AURA_UUID      "beb5483e-36e1-4688-b7f5-ea07361b26aa"
#define CHAR_AUDIO_UUID     "beb5483e-36e1-4688-b7f5-ea07361b26ab"
#define CHAR_FINDER_UUID    "beb5483e-36e1-4688-b7f5-ea07361b26ac"
#define CHAR_GPS_UUID       "beb5483e-36e1-4688-b7f5-ea07361b26ad"
#define CHAR_SYNC_UUID      "beb5483e-36e1-4688-b7f5-ea07361b26ae"
#define CHAR_NOTIFY_UUID    "beb5483e-36e1-4688-b7f5-ea07361b26af"

// ─────────────────────────────────────────────────────────────────────────────
// OLED
// ─────────────────────────────────────────────────────────────────────────────
#define OLED_ADDR        0x3C
#define SCREEN_WIDTH     128
#define SCREEN_HEIGHT    64

// ─────────────────────────────────────────────────────────────────────────────
// HMC5883L
// ─────────────────────────────────────────────────────────────────────────────
#define HMC5883L_ADDR    0x1E
#define HMC5883L_REG_A   0x00
#define HMC5883L_REG_MODE 0x02
#define HMC5883L_REG_DATA 0x03

// ─────────────────────────────────────────────────────────────────────────────
// AURA MODES
// ─────────────────────────────────────────────────────────────────────────────
enum AuraMode { AURA_OFF = 0, AURA_CALM = 1, AURA_RHYTHM = 2, AURA_ENERGY = 3 };

// Audio visualizer modes / palettes / band focus (must match app indices)
enum VizMode  { VIZ_BARS = 0, VIZ_PULSE = 1, VIZ_SPARKLE = 2, VIZ_SPECTRUM = 3 };
enum BandFocus{ BAND_BASS = 0, BAND_MIDS = 1, BAND_HIGHS = 2 };

// ─────────────────────────────────────────────────────────────────────────────
// GLOBAL STATE
// ─────────────────────────────────────────────────────────────────────────────
struct AppState {
  uint8_t  r = 255, g = 0, b = 128;
  uint8_t  brightness = 191;
  AuraMode aura = AURA_OFF;
  // audio
  bool     audioOn = false;
  uint8_t  audioSensitivity = 165;
  uint8_t  vizMode = VIZ_BARS;
  uint8_t  palette = 0;            // 0=holi 1=hotpink 2=fire 3=ice
  uint8_t  smoothing = 128;        // 0-255  (higher = slower/smoother response)
  uint8_t  bandFocus = BAND_BASS;
  // mesh
  bool     finderOn = false;
  float    gpsLat = 0.0f, gpsLon = 0.0f;
  bool     syncActive = false;
  uint8_t  syncR = 0, syncG = 0, syncB = 0;
  AuraMode syncAura = AURA_OFF;
  bool     connected = false;
};

AppState state;
bool bleConnected = false;

// ─────────────────────────────────────────────────────────────────────────────
// HARDWARE OBJECTS
// ─────────────────────────────────────────────────────────────────────────────
CRGB              leds[NEOPIXEL_COUNT];
Adafruit_SSD1306  display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

#if ENABLE_LORA
  SX1262 lora(new Module(LORA_CS, LORA_DIO1, LORA_RST, LORA_BUSY));
#endif

#if ENABLE_AUDIO
  #define FFT_SAMPLES  256
  #define SAMPLE_RATE  16000
  static i2s_chan_handle_t i2s_rx_handle = NULL;
  double fftReal[FFT_SAMPLES];
  double fftImag[FFT_SAMPLES];
  ArduinoFFT FFT = ArduinoFFT(fftReal, fftImag, FFT_SAMPLES, SAMPLE_RATE);
#endif

// ─────────────────────────────────────────────────────────────────────────────
// VISUALIZER PALETTES  (match app's VIZ_PALETTES stops)
// ─────────────────────────────────────────────────────────────────────────────
struct PaletteDef { const CRGB *colors; uint8_t len; };

const CRGB PAL_HOLI[]    = { CRGB(0xFF4500), CRGB(0xFFD23F), CRGB(0x8A2BE2), CRGB(0x22D3EE), CRGB(0xA3FF3D) };
const CRGB PAL_HOTPINK[] = { CRGB(0xFF3DA5), CRGB(0xFFFAF1) };
const CRGB PAL_FIRE[]    = { CRGB(0xFFFF00), CRGB(0xFFA500), CRGB(0xFF4500), CRGB(0xC71585) };
const CRGB PAL_ICE[]     = { CRGB(0x22D3EE), CRGB(0x0000FF), CRGB(0x9B5DE5) };

const PaletteDef PALETTES[4] = {
  { PAL_HOLI,    5 },
  { PAL_HOTPINK, 2 },
  { PAL_FIRE,    4 },
  { PAL_ICE,     3 },
};

// pos 0.0–1.0 across the palette gradient
CRGB paletteColor(uint8_t idx, float pos) {
  if (idx > 3) idx = 0;
  const PaletteDef &p = PALETTES[idx];
  if (p.len == 1) return p.colors[0];
  if (pos < 0) pos = 0; if (pos > 1) pos = 1;
  float fp = pos * (p.len - 1);
  int   i  = (int)fp;
  if (i >= p.len - 1) return p.colors[p.len - 1];
  uint8_t frac = (uint8_t)((fp - i) * 255.0f);
  return blend(p.colors[i], p.colors[i + 1], frac);
}

// NimBLE notify char
NimBLECharacteristic *pNotifyChar = nullptr;

// Timing
unsigned long lastNeoUpdate  = 0;
unsigned long lastOledUpdate = 0;
unsigned long lastMagUpdate  = 0;
unsigned long lastLoraUpdate = 0;
unsigned long connectedAt    = 0;

float compassHeading = 0.0f;

// LCG for pseudo-random aura patterns
uint32_t rngState = 12345;
uint32_t lcgNext() {
  rngState = rngState * 1664525UL + 1013904223UL;
  return rngState;
}

// ─────────────────────────────────────────────────────────────────────────────
// FORWARD DECLARATIONS  (needed because callbacks reference these functions)
// ─────────────────────────────────────────────────────────────────────────────
void oledShowWaiting();
void oledShowConnected();
void oledShowAudioMode();
void oledShowCompass(float heading, float targetBearing, const char *targetName, float distM);

// ─────────────────────────────────────────────────────────────────────────────
// BLE CALLBACKS
// ─────────────────────────────────────────────────────────────────────────────
class ServerCallbacks : public NimBLEServerCallbacks {
  void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo) override {
    bleConnected    = true;
    connectedAt     = millis();
    state.connected = true;
    tone(BUZZER_PIN, 880,  80); delay(100);
    tone(BUZZER_PIN, 1320, 80);
    oledShowConnected();
  }
  void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason) override {
    bleConnected    = false;
    state.connected = false;
    NimBLEDevice::getAdvertising()->start();  // NimBLE 2.x
    oledShowWaiting();
  }
};

class ColorCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 3) {
      state.r = (uint8_t)val[0];
      state.g = (uint8_t)val[1];
      state.b = (uint8_t)val[2];
      state.audioOn = false;
    }
  }
};

class BrightnessCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 1) state.brightness = (uint8_t)val[0];
  }
};

class AuraCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 1) {
      uint8_t v     = (uint8_t)val[0];
      state.aura    = (v <= 3) ? (AuraMode)v : AURA_OFF;
      state.audioOn = false;
    }
  }
};

// Audio config — 6-byte payload: [on, sensitivity, vizMode, palette, smoothing, bandFocus]
// Parsed defensively so older 2-byte writes still work.
class AudioCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 1) state.audioOn         = (val[0] != 0);
    if (val.size() >= 2) state.audioSensitivity = (uint8_t)val[1];
    if (val.size() >= 3) state.vizMode          = (val[2] <= 3) ? (uint8_t)val[2] : 0;
    if (val.size() >= 4) state.palette          = (val[3] <= 3) ? (uint8_t)val[3] : 0;
    if (val.size() >= 5) state.smoothing        = (uint8_t)val[4];
    if (val.size() >= 6) state.bandFocus        = (val[5] <= 2) ? (uint8_t)val[5] : 0;
    if (state.audioOn) state.aura = AURA_OFF;
  }
};

class FinderCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 1) state.finderOn = (val[0] != 0);
  }
};

class GpsCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() < 3) return;
    char buf[32] = {0};
    size_t len = val.size() < 31 ? val.size() : 31;
    for (size_t i = 0; i < len; i++) buf[i] = (char)val[i];
    char *comma = strchr(buf, ',');
    if (comma) {
      *comma = '\0';
      state.gpsLat = atof(buf);
      state.gpsLon = atof(comma + 1);
    }
  }
};

class SyncCallback : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override {
    auto val = pChar->getValue();
    if (val.size() >= 4) {
      state.syncR    = (uint8_t)val[0];
      state.syncG    = (uint8_t)val[1];
      state.syncB    = (uint8_t)val[2];
      state.syncAura = (AuraMode)((uint8_t)val[3] <= 3 ? (uint8_t)val[3] : 0);
      state.syncActive = true;
      state.r = state.syncR; state.g = state.syncG; state.b = state.syncB;
      state.aura = state.syncAura;
      tone(BUZZER_PIN, 1760, 150);
    }
  }
};

// ─────────────────────────────────────────────────────────────────────────────
// OLED HELPERS
// ─────────────────────────────────────────────────────────────────────────────
void oledShowWaiting() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(4, 2);  display.println("Enable Bluetooth");
  display.setCursor(4, 14); display.println("& Connect");
  display.setTextSize(2);
  display.setCursor(4, 30); display.println(DEVICE_NAME);
  display.display();
}

void oledShowConnected() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2);
  display.setCursor(8, 18);  display.println("Connected");
  display.setTextSize(1);
  display.setCursor(24, 46); display.println(DEVICE_NAME);
  display.display();
}

void oledShowAudioMode() {
  const char *vizNames[]  = { "Bars", "Pulse", "Sparkle", "Spectrum" };
  const char *palNames[]  = { "Holi", "HotPink", "Fire", "Ice" };
  const char *bandNames[] = { "Bass", "Mids", "Highs" };
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(4, 2);  display.print("BEAT SYNC  ");
  display.println(vizNames[state.vizMode <= 3 ? state.vizMode : 0]);
  display.setCursor(4, 14);
  display.print(palNames[state.palette <= 3 ? state.palette : 0]);
  display.print(" / ");
  display.println(bandNames[state.bandFocus <= 2 ? state.bandFocus : 0]);
  int barW = map(state.audioSensitivity, 0, 255, 0, 120);
  display.drawRect(4, 30, 120, 10, SSD1306_WHITE);
  display.fillRect(4, 30, barW, 10, SSD1306_WHITE);
  display.setCursor(4, 46);
  display.print("Sens: ");
  display.print(map(state.audioSensitivity, 0, 255, 0, 100));
  display.print("%");
  display.display();
}

void oledShowCompass(float heading, float targetBearing, const char *targetName, float distM) {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  const int cx = 96, cy = 32, cr = 28;
  display.drawCircle(cx, cy, cr, SSD1306_WHITE);
  display.setCursor(cx - 2, cy - cr - 8); display.print("N");
  display.setCursor(cx - 2, cy + cr + 1); display.print("S");
  display.setCursor(cx + cr + 1, cy - 3); display.print("E");
  display.setCursor(cx - cr - 7, cy - 3); display.print("W");
  float relRad = ((targetBearing - heading) + 360.0f) * DEG_TO_RAD;
  int ax = cx + (int)((cr - 5) * sinf(relRad));
  int ay = cy - (int)((cr - 5) * cosf(relRad));
  display.drawLine(cx, cy, ax, ay, SSD1306_WHITE);
  display.fillCircle(ax, ay, 3, SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, 0);  display.println("POINTING TO:");
  display.println(targetName);
  char buf[16];
  snprintf(buf, sizeof(buf), "%.1f m", distM);
  display.println(buf);
  display.display();
}

void oledShowStatus() {
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(4, 2);  display.println(DEVICE_NAME);
  const char *auraNames[] = { "Steady", "Calm", "Rhythm", "Energy" };
  display.setCursor(4, 14); display.print("Aura: "); display.println(auraNames[state.aura]);
  char buf[20];
  snprintf(buf, sizeof(buf), "RGB #%02X%02X%02X", state.r, state.g, state.b);
  display.setCursor(4, 26); display.println(buf);
  display.setCursor(4, 38); display.print("Bri: ");
  display.print((int)(state.brightness * 100 / 255));
  display.println("%");
  display.display();
}

// ─────────────────────────────────────────────────────────────────────────────
// MAGNETOMETER (disabled until ENABLE_MAG = true)
// ─────────────────────────────────────────────────────────────────────────────
#if ENABLE_MAG
bool magInit() {
  Wire.beginTransmission(HMC5883L_ADDR);
  Wire.write(HMC5883L_REG_A);
  Wire.write(0x70);  // 8-sample avg, 15Hz
  Wire.write(0xA0);  // gain
  Wire.write(0x00);  // continuous
  return Wire.endTransmission() == 0;
}

float magReadHeading() {
  Wire.beginTransmission(HMC5883L_ADDR);
  Wire.write(HMC5883L_REG_DATA);
  Wire.endTransmission();
  Wire.requestFrom((uint8_t)HMC5883L_ADDR, (uint8_t)6);
  if (Wire.available() < 6) return compassHeading;
  int16_t x = (Wire.read() << 8) | Wire.read();
  int16_t z = (Wire.read() << 8) | Wire.read();
  int16_t y = (Wire.read() << 8) | Wire.read();
  float h = atan2f((float)y, (float)x) * RAD_TO_DEG;
  if (h < 0) h += 360.0f;
  return h;
}
#endif

// ─────────────────────────────────────────────────────────────────────────────
// AUDIO REACTIVE (disabled until ENABLE_AUDIO = true)
// ─────────────────────────────────────────────────────────────────────────────
#if ENABLE_AUDIO
void i2sInit() {
  // New ESP-IDF 5.x I2S driver — compatible with FastLED on ESP32-S3
  i2s_chan_config_t chan_cfg  = I2S_CHANNEL_DEFAULT_CONFIG(I2S_NUM_0, I2S_ROLE_MASTER);
  chan_cfg.dma_desc_num  = 4;
  chan_cfg.dma_frame_num = FFT_SAMPLES;  // 256 frames per DMA buffer
  ESP_ERROR_CHECK(i2s_new_channel(&chan_cfg, NULL, &i2s_rx_handle));

  i2s_std_config_t std_cfg = {
    .clk_cfg  = I2S_STD_CLK_DEFAULT_CONFIG(SAMPLE_RATE),
    // STEREO mode: generates proper alternating WS that INMP441 requires.
    // L/R=GND → mic outputs on left channel (WS low). We extract both channels
    // and use whichever has signal (debug shows peakL vs peakR).
    .slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(I2S_DATA_BIT_WIDTH_32BIT, I2S_SLOT_MODE_STEREO),
    .gpio_cfg = {
      .mclk = I2S_GPIO_UNUSED,
      .bclk = (gpio_num_t)I2S_SCK_PIN,
      .ws   = (gpio_num_t)I2S_WS_PIN,
      .dout = I2S_GPIO_UNUSED,
      .din  = (gpio_num_t)I2S_SD_PIN,
      .invert_flags = { .mclk_inv = false, .bclk_inv = false, .ws_inv = false },
    },
  };
  ESP_ERROR_CHECK(i2s_channel_init_std_mode(i2s_rx_handle, &std_cfg));
  ESP_ERROR_CHECK(i2s_channel_enable(i2s_rx_handle));
}

// Map band focus -> FFT bin range. Bin width = SAMPLE_RATE/FFT_SAMPLES = 62.5 Hz.
void bandRange(int &loBin, int &hiBin) {
  switch (state.bandFocus) {
    case BAND_BASS:  loBin = 1;  hiBin = 6;   break;  // ~60–375 Hz
    case BAND_MIDS:  loBin = 6;  hiBin = 40;  break;  // ~375–2500 Hz
    default:         loBin = 40; hiBin = 120; break;  // ~2.5–7.5 kHz
  }
}

void audioReactiveUpdate() {
  static unsigned long lastDbg  = 0;
  static int32_t       lastPeak = 0;

  // Stereo buffer: FFT_SAMPLES L/R pairs interleaved = 2× samples
  int32_t raw[FFT_SAMPLES * 2];
  size_t  bytesRead = 0;
  // 40 ms timeout — 256 stereo frames @ 16 kHz = 32 ms
  i2s_channel_read(i2s_rx_handle, raw, sizeof(raw), &bytesRead, pdMS_TO_TICKS(40));

  int nPairs = (int)(bytesRead / sizeof(int32_t)) / 2;

  // Measure both channel peaks — tells us which channel the INMP441 is using
  int32_t peakL = 0, peakR = 0;
  for (int i = 0; i < nPairs; i++) {
    int32_t avL = raw[i*2]   < 0 ? -raw[i*2]   : raw[i*2];
    int32_t avR = raw[i*2+1] < 0 ? -raw[i*2+1] : raw[i*2+1];
    if (avL > peakL) peakL = avL;
    if (avR > peakR) peakR = avR;
  }
  lastPeak = peakL > peakR ? peakL : peakR;

  // Extract left channel (even indices, L/R=GND → left = WS low)
  // If peakR > peakL in debug, swap to raw[i*2+1] for right channel
  for (int i = 0; i < FFT_SAMPLES; i++) {
    fftReal[i] = (i < nPairs) ? (double)(raw[i*2] >> 8) : 0.0;
    fftImag[i] = 0.0;
  }
  FFT.windowing(FFTWindow::Hamming, FFTDirection::Forward);
  FFT.compute(FFTDirection::Forward);
  FFT.complexToMagnitude();

  float gain = 0.3f + (state.audioSensitivity / 255.0f) * 3.0f;
  float sm   = (state.smoothing / 255.0f) * 0.9f;

  int loBin, hiBin; bandRange(loBin, hiBin);

  double sum = 0;
  for (int b = loBin; b < hiBin && b < FFT_SAMPLES / 2; b++) sum += fftReal[b];
  float rawLevel = (float)(sum / (hiBin - loBin) * gain / 500000.0f);
  float level    = constrain(rawLevel, 0.0f, 1.0f);

  static float sLevel = 0.0f;
  sLevel = sLevel * sm + level * (1.0f - sm);

  // Debug every 2 s — clap near mic and watch rawPeak jump
  if (millis() - lastDbg > 2000) {
    lastDbg = millis();
    Serial.printf("[Audio] bytes=%u  peakL=%d  peakR=%d  rawLevel=%.5f  sLevel=%.4f  viz=%d\n",
                  (unsigned)bytesRead, peakL, peakR, rawLevel, sLevel, state.vizMode);
  }

  // Idle animation — full brightness palette drift when no signal.
  // Viz/palette switches are always visible here even without a mic.
  if (sLevel < 0.04f) {
    static float idlePhase = 0.0f;
    idlePhase = fmodf(idlePhase + 0.003f, 1.0f);
    for (int i = 0; i < NEOPIXEL_COUNT; i++) {
      float t = fmodf(idlePhase + (float)i / NEOPIXEL_COUNT, 1.0f);
      leds[i] = paletteColor(state.palette, t);   // full brightness — clearly visible
    }
    FastLED.setBrightness(state.brightness);
    FastLED.show();
    return;
  }

  switch (state.vizMode) {

    case VIZ_BARS: {                       // VU meter — fill from one end
      int lit = (int)(sLevel * NEOPIXEL_COUNT + 0.5f);
      for (int i = 0; i < NEOPIXEL_COUNT; i++)
        leds[i] = (i < lit) ? paletteColor(state.palette, (float)i / NEOPIXEL_COUNT)
                            : CRGB::Black;
      break;
    }

    case VIZ_PULSE: {
      // Concentric ring expansion — all pixels in each ring turn on together.
      // Low sound  → center only (1 px front + 1 px back)
      // Medium     → center + inner ring (4 px each side)
      // Loud       → all 3 rings (17 px each side)
      //
      // Tune these thresholds if rings trigger too early or too late:
      const float T_CENTER = 0.06f;
      const float T_INNER  = 0.30f;
      const float T_OUTER  = 0.65f;

      // Smooth ramp: 0→full brightness over 0.15 window above each threshold.
      // This gives a crisp but not jarring snap-on.
      auto ringBri = [](float lvl, float thr) -> uint8_t {
        if (lvl <= thr) return 0;
        return (uint8_t)(constrain((lvl - thr) / 0.15f, 0.f, 1.f) * 255.f);
      };
      uint8_t bC = ringBri(sLevel, T_CENTER);
      uint8_t bI = ringBri(sLevel, T_INNER);
      uint8_t bO = ringBri(sLevel, T_OUTER);

      // Ring colours from active palette
      CRGB cCenter = paletteColor(state.palette, 0.5f);
      CRGB cInner  = paletteColor(state.palette, 0.3f);
      CRGB cOuter  = paletteColor(state.palette, 0.8f);

      // ── Front ──────────────────────────────────────────────
      leds[FRONT_CENTER] = cCenter; leds[FRONT_CENTER].nscale8(bC);
      for (int i = FRONT_INNER_START; i <= FRONT_INNER_END; i++)
        { leds[i] = cInner; leds[i].nscale8(bI); }
      for (int i = FRONT_OUTER_START; i <= FRONT_OUTER_END; i++)
        { leds[i] = cOuter; leds[i].nscale8(bO); }

      // ── Back ───────────────────────────────────────────────
      leds[BACK_CENTER] = cCenter; leds[BACK_CENTER].nscale8(bC);
      for (int i = BACK_INNER_START; i <= BACK_INNER_END; i++)
        { leds[i] = cInner; leds[i].nscale8(bI); }
      for (int i = BACK_OUTER_START; i <= BACK_OUTER_END; i++)
        { leds[i] = cOuter; leds[i].nscale8(bO); }

      break;
    }

    case VIZ_SPARKLE: {                    // random flashes scaled by loudness
      fadeToBlackBy(leds, NEOPIXEL_COUNT, 48);
      int sparks = (int)(sLevel * NEOPIXEL_COUNT);
      for (int s = 0; s < sparks; s++) {
        int idx = lcgNext() % NEOPIXEL_COUNT;
        leds[idx] = paletteColor(state.palette, (lcgNext() % 256) / 255.0f);
      }
      break;
    }

    default: {
      // VIZ_SPECTRUM — each LED maps to a distinct frequency bin inside the band.
      // Uses ratio mapping so it works correctly even when band has fewer bins than LEDs (e.g. Bass).
      static float sBin[NEOPIXEL_COUNT] = {};
      static uint8_t lastBand = 255;
      if (state.bandFocus != lastBand) {   // reset EMA when band changes
        memset(sBin, 0, sizeof(sBin));
        lastBand = state.bandFocus;
      }
      for (int led = 0; led < NEOPIXEL_COUNT; led++) {
        float frac = (NEOPIXEL_COUNT > 1) ? (float)led / (NEOPIXEL_COUNT - 1) : 0.5f;
        int   bin  = loBin + (int)(frac * (hiBin - loBin));
        bin = constrain(bin, 0, FFT_SAMPLES / 2 - 1);
        float norm = constrain((float)(fftReal[bin] * gain / 2000.0f), 0.0f, 1.0f);
        sBin[led]  = sBin[led] * sm + norm * (1.0f - sm);
        CRGB c = paletteColor(state.palette, frac);
        c.nscale8((uint8_t)(sBin[led] * 255));
        leds[led] = c;
      }
      break;
    }
  }

  FastLED.setBrightness(state.brightness);
  FastLED.show();
}
#endif

// ─────────────────────────────────────────────────────────────────────────────
// NEOPIXEL PATTERNS  (color / aura — FastLED, master brightness applied by caller)
// ─────────────────────────────────────────────────────────────────────────────
void neoSteady() {
  fill_solid(leds, NEOPIXEL_COUNT, CRGB(state.r, state.g, state.b));
  FastLED.show();
}

void neoCalmUpdate(unsigned long /*now*/) {
  static float phase = 0.0f;
  phase += 0.005f;
  for (int i = 0; i < NEOPIXEL_COUNT; i++) {
    float hue = fmodf(phase + (float)i / NEOPIXEL_COUNT, 1.0f);
    float amp = 0.5f + 0.5f * sinf((phase * TWO_PI * 0.6f) + (float)i / NEOPIXEL_COUNT * TWO_PI);
    leds[i] = CHSV((uint8_t)(hue * 255), 255, (uint8_t)(amp * 255));
  }
  FastLED.show();
}

void neoRhythmUpdate(unsigned long now) {
  static unsigned long lastTick = 0;
  static CRGB cols[NEOPIXEL_COUNT];
  static bool ledOn[NEOPIXEL_COUNT];
  if (now - lastTick > 450) {
    lastTick = now;
    for (int i = 0; i < NEOPIXEL_COUNT; i++) {
      ledOn[i] = (lcgNext() % 2 == 0);
      cols[i]  = CHSV((uint8_t)(lcgNext() % 256), 255, 255);
    }
  }
  for (int i = 0; i < NEOPIXEL_COUNT; i++)
    leds[i] = ledOn[i] ? cols[i] : CRGB::Black;
  FastLED.show();
}

void neoEnergyUpdate(unsigned long now) {
  static unsigned long lastTick = 0;
  if (now - lastTick < 80) return;
  lastTick = now;
  for (int i = 0; i < NEOPIXEL_COUNT; i++) {
    if ((lcgNext() % 10) < 4)
      leds[i] = CHSV((uint8_t)(lcgNext() % 256), 255, 255);
    else
      leds[i] = CRGB::Black;
  }
  FastLED.show();
}

// ─────────────────────────────────────────────────────────────────────────────
// LORA MESH (disabled until ENABLE_LORA = true)
// ─────────────────────────────────────────────────────────────────────────────
#if ENABLE_LORA
struct MeshNode { char name[16]; float lat, lon; unsigned long lastSeen; };
#define MAX_NODES 8
MeshNode meshNodes[MAX_NODES];
int meshNodeCount = 0;

void loraTxPosition() {
  if (!state.finderOn) return;
  char pkt[64];
  snprintf(pkt, sizeof(pkt), "RB:%s:%.6f:%.6f", DEVICE_NAME, state.gpsLat, state.gpsLon);
  lora.transmit((uint8_t *)pkt, strlen(pkt));
}

void loraRxCheck() {
  if (!state.finderOn) return;
  uint8_t buf[64]; int len = sizeof(buf);
  int err = lora.receive(buf, len);
  if (err == RADIOLIB_ERR_NONE) {
    buf[len] = 0;
    char *s = (char*)buf;
    if (strncmp(s, "RB:", 3) == 0) {
      char *c1 = strchr(s + 3, ':');
      if (!c1) return;
      char *c2 = strchr(c1 + 1, ':');
      if (!c2) return;
      *c1 = '\0'; *c2 = '\0';
      const char *name = s + 3;
      float lat = atof(c1 + 1), lon = atof(c2 + 1);
      for (int i = 0; i < meshNodeCount; i++) {
        if (strcmp(meshNodes[i].name, name) == 0) {
          meshNodes[i].lat = lat; meshNodes[i].lon = lon;
          meshNodes[i].lastSeen = millis(); return;
        }
      }
      if (meshNodeCount < MAX_NODES) {
        strncpy(meshNodes[meshNodeCount].name, name, 15);
        meshNodes[meshNodeCount].lat = lat; meshNodes[meshNodeCount].lon = lon;
        meshNodes[meshNodeCount].lastSeen = millis();
        meshNodeCount++;
      }
    }
  }
}

float bearingTo(float la1, float lo1, float la2, float lo2) {
  float dLon = (lo2 - lo1) * DEG_TO_RAD;
  float y = sinf(dLon) * cosf(la2 * DEG_TO_RAD);
  float x = cosf(la1 * DEG_TO_RAD) * sinf(la2 * DEG_TO_RAD)
           - sinf(la1 * DEG_TO_RAD) * cosf(la2 * DEG_TO_RAD) * cosf(dLon);
  return fmodf(atan2f(y, x) * RAD_TO_DEG + 360.0f, 360.0f);
}
float distanceTo(float la1, float lo1, float la2, float lo2) {
  const float R = 6371000.0f;
  float dLat = (la2 - la1) * DEG_TO_RAD, dLon = (lo2 - lo1) * DEG_TO_RAD;
  float a = sinf(dLat/2)*sinf(dLat/2) + cosf(la1*DEG_TO_RAD)*cosf(la2*DEG_TO_RAD)*sinf(dLon/2)*sinf(dLon/2);
  return R * 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a));
}
#endif

// ─────────────────────────────────────────────────────────────────────────────
// BLE NOTIFY — compass heading → app
// ─────────────────────────────────────────────────────────────────────────────
void notifyHeading() {
  if (!bleConnected || !pNotifyChar) return;
  uint16_t h = (uint16_t)compassHeading;
  pNotifyChar->setValue((uint8_t*)&h, 2);
  pNotifyChar->notify();
}

// ─────────────────────────────────────────────────────────────────────────────
// SETUP
// ─────────────────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);
  delay(200);
  Serial.println("\n=== Rangbheru " DEVICE_NAME " ===");

  // Buzzer boot beep
  pinMode(BUZZER_PIN, OUTPUT);
  tone(BUZZER_PIN, 440, 100); delay(150);
  tone(BUZZER_PIN, 660, 100); delay(150);

  // I2C
  Wire.begin(I2C_SDA, I2C_SCL);

  // OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println("OLED not found — check wiring");
  } else {
    display.clearDisplay(); display.display();
    Serial.println("OLED OK");
  }
  oledShowWaiting();

  // Magnetometer
#if ENABLE_MAG
  if (!magInit()) Serial.println("HMC5883L not found");
  else            Serial.println("Magnetometer OK");
#endif

  // NeoPixels (FastLED with current cap)
  FastLED.addLeds(leds, NEOPIXEL_COUNT);
  FastLED.setMaxPowerInVoltsAndMilliamps(5, MAX_MA);
  FastLED.setBrightness(180);
  FastLED.clear(); FastLED.show();
  // Boot rainbow sweep
  for (int i = 0; i < NEOPIXEL_COUNT; i++) {
    leds[i] = CHSV((uint8_t)(i * 255 / NEOPIXEL_COUNT), 255, 255);
    FastLED.show(); delay(30);
  }
  delay(300);
  FastLED.clear(); FastLED.show();
  Serial.println("NeoPixels OK (FastLED)");

  // Audio
#if ENABLE_AUDIO
  i2sInit();
  Serial.println("I2S mic OK");
#endif

  // LoRa
#if ENABLE_LORA
  SPI.begin(LORA_SCK, LORA_MISO, LORA_MOSI, LORA_CS);
  int loraErr = lora.begin(868.0);
  if (loraErr != RADIOLIB_ERR_NONE) {
    Serial.printf("LoRa failed: %d\n", loraErr);
  } else {
    lora.setSpreadingFactor(7);
    lora.setBandwidth(125.0);
    lora.setCodingRate(5);
    lora.setOutputPower(14);
    Serial.println("LoRa OK");
  }
#endif

  // BLE
  NimBLEDevice::init(DEVICE_NAME);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);

  NimBLEServer  *pServer  = NimBLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());

  NimBLEService *pService = pServer->createService(SERVICE_UUID);

  const uint32_t W = NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR;
  const uint32_t N = NIMBLE_PROPERTY::NOTIFY;

  pService->createCharacteristic(CHAR_COLOR_UUID,   W)->setCallbacks(new ColorCallback());
  pService->createCharacteristic(CHAR_BRI_UUID,     W)->setCallbacks(new BrightnessCallback());
  pService->createCharacteristic(CHAR_AURA_UUID,    W)->setCallbacks(new AuraCallback());
  pService->createCharacteristic(CHAR_AUDIO_UUID,   W)->setCallbacks(new AudioCallback());
  pService->createCharacteristic(CHAR_FINDER_UUID,  W)->setCallbacks(new FinderCallback());
  pService->createCharacteristic(CHAR_GPS_UUID,     W)->setCallbacks(new GpsCallback());
  pService->createCharacteristic(CHAR_SYNC_UUID,    W)->setCallbacks(new SyncCallback());
  pNotifyChar = pService->createCharacteristic(CHAR_NOTIFY_UUID, N);

  pService->start();

  // NimBLE 2.x advertising setup
  NimBLEAdvertising *pAdv = NimBLEDevice::getAdvertising();
  pAdv->setName(DEVICE_NAME);           // broadcast name so Android can find it
  pAdv->addServiceUUID(SERVICE_UUID);   // include service UUID in advertisement
  pAdv->setMinInterval(32);             // 20ms — fast advertising, easier to discover
  pAdv->setMaxInterval(64);             // 40ms
  pAdv->start();                        // NimBLE 2.x: use pAdv->start(), not NimBLEDevice::startAdvertising()

  Serial.println("BLE advertising — ready to connect");
  Serial.print("Device name: "); Serial.println(DEVICE_NAME);
}

// ─────────────────────────────────────────────────────────────────────────────
// LOOP
// ─────────────────────────────────────────────────────────────────────────────
void loop() {
  unsigned long now = millis();

  // Magnetometer
#if ENABLE_MAG
  if (now - lastMagUpdate >= 50) {
    lastMagUpdate = now;
    compassHeading = magReadHeading();
    if (bleConnected) notifyHeading();
  }
#endif

  // Audio reactive mode
#if ENABLE_AUDIO
  if (state.audioOn) {
    audioReactiveUpdate();
    if (now - lastOledUpdate >= 1000) { lastOledUpdate = now; oledShowAudioMode(); }
    return;
  }
#endif

  // NeoPixel patterns (color / aura)
  if (now - lastNeoUpdate >= 16) {
    lastNeoUpdate = now;
    FastLED.setBrightness(state.brightness);   // master brightness = app slider
    switch (state.aura) {
      case AURA_CALM:   neoCalmUpdate(now);   break;
      case AURA_RHYTHM: neoRhythmUpdate(now); break;
      case AURA_ENERGY: neoEnergyUpdate(now); break;
      default:          neoSteady();          break;
    }
  }

  // OLED refresh
  if (now - lastOledUpdate >= 500) {
    lastOledUpdate = now;
    if (!bleConnected) {
      oledShowWaiting();
    } else if (now - connectedAt < 5000) {
      oledShowConnected();
#if ENABLE_LORA
    } else if (state.finderOn && meshNodeCount > 0) {
      MeshNode &n = meshNodes[0];
      oledShowCompass(compassHeading,
                      bearingTo(state.gpsLat, state.gpsLon, n.lat, n.lon),
                      n.name,
                      distanceTo(state.gpsLat, state.gpsLon, n.lat, n.lon));
    } else if (state.finderOn) {
      display.clearDisplay();
      display.setTextColor(SSD1306_WHITE); display.setTextSize(1);
      display.setCursor(4, 2);  display.println("Friend Finder ON");
      display.setCursor(4, 14); display.println("Searching mesh...");
      display.display();
#endif
    } else {
      oledShowStatus();
    }
  }

  // LoRa mesh
#if ENABLE_LORA
  if (now - lastLoraUpdate >= 5000) { lastLoraUpdate = now; loraTxPosition(); }
  loraRxCheck();
#endif
}